#ifndef _MAE378_MOTORS_H_
#define _MAE378_MOTORS_H_ 1

#include "mae378_defines.h"



// Directional Ops-------------------------------------------------------------

void goForward()
{
	leftWheelForward();
	rightWheelForward();
	//if(DEBUG) printf("Moving forward\n");
}

void goBackward()
{
	leftWheelBackward();
	rightWheelBackward();
	//if(DEBUG) printf("Moving backward\n");
}

void stop()
{
	leftWheelStop();
	rightWheelStop();
	//if(DEBUG) printf("Stopping\n");
}

void turnLeft()
{
	leftWheelBackward();
	rightWheelForward();
	//if(DEBUG) printf("Turning left\n");
}

void turnRight()
{
	leftWheelForward();
	rightWheelBackward();
	//if(DEBUG) printf("Turning right\n");
}


// Left Wheel Functions--------------------------------------------------------

void leftWheelForward()
{
	clearPin(LWM_P1);
	setPin(LWM_P0);
}

void leftWheelBackward()
{
	clearPin(LWM_P0);
	setPin(LWM_P1);
}

void leftWheelStop()
{
	clearPin(LWM_P0);
	clearPin(LWM_P1);
}


// Right Wheel Functions-------------------------------------------------------

void rightWheelForward()
{
	clearPin(RWM_P0);
	setPin(RWM_P1);
}

void rightWheelBackward()
{
	clearPin(RWM_P1);
	setPin(RWM_P0);
}

void rightWheelStop()
{
	clearPin(RWM_P0);
	clearPin(RWM_P1);
}


// Platform Motor Functions----------------------------------------------------

void platformUp()
{
	clearPin(PM_P0);
	setPin(PM_P1);
}

void platformDown()
{
	clearPin(PM_P1);
	setPin(PM_P0);
}

void platformStop()
{
	clearPin(PM_P0);
	clearPin(PM_P1);
}



void allStop()
{
	leftWheelStop();
	rightWheelStop();
	platformStop();
	//if(DEBUG) printf("Stopping all\n");
}



#endif	// _MAE378_MOTORS_H_
